﻿/**
 * This class seperates the tracking of skeletons from the KinectControl class.
 * Basically it contains the joint tracking and drawing of multiple skeletons
 * */


using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Shapes;

namespace EHU_InterfaceProgramming
{
    public class PersonTracker
    {


        private const int SKELETON_COUNT = 6;
        public Skeleton[] allSkeletons = new Skeleton[SKELETON_COUNT];

        public Person person1;
        public Person person2;

        // Keeps track of the main window object reference
        MainWindow window;

        // Keeps track of the sensor object for the kinect for accessing it later
        KinectSensor sensor;

        public PersonTracker(MainWindow window, KinectSensor sensor)
        {
            // Assign the object reference window to the variable so we can track it later
            this.window = window; 

            // Assign the object reference to the sensor variable for accessing it later
            this.sensor = sensor;
        }

        public void initialisePerson(int id)
        {
            // Person ID  0 - Assign the person1 variable to this person
            // and intiialise the joint and skeleton tracking
            if (id == 0)
            {
                person1 = new Person(id);
                List<Ellipse> p1Joints = person1.getJoints();
                foreach (Ellipse Joint in p1Joints)
                {
                    window.canDraw.Children.Add(Joint);
                }

                List<Line> p1Bones = person1.getBones();
                foreach (Line Bone in p1Bones)
                {
                    window.canDraw.Children.Add(Bone);
                }
            }
            // Person ID 1 - assign the person2 variable and initialise
            // the joint and skeleton tracking
            else if (id == 1)
            {
                person2 = new Person(id);
                List<Ellipse> p2Joints = person2.getJoints();
                foreach (Ellipse Joint in p2Joints)
                {
                    window.canDraw.Children.Add(Joint);
                }

                List<Line> p2Bones = person2.getBones();
                foreach (Line Bone in p2Bones)
                {
                    window.canDraw.Children.Add(Bone);
                }
            }
        }

        public void GetSkeleton(AllFramesReadyEventArgs e, ref Skeleton me, ref Skeleton notMe)
        {
            try
            {
                // Fixes "Warning, a skeleton frame was not disposed"
                using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
                {
           //         window.Log("getSkeleton called.");
                    
                    skeletonFrameData.CopySkeletonDataTo(allSkeletons);

         //           window.Log("CopySkeletonDataTo called.");
     

                    // me represents ONE person
                    // noteME represents the other person
                    // Create a list for storing multiple people and only reference
                    // the first 2 people of that list.
                    List<Skeleton> person = new List<Skeleton>();

                     person = (from s in allSkeletons where s.TrackingState == SkeletonTrackingState.Tracked select s).Distinct().ToList();

                  // Update the number of skeletons text here
                    window.NumberOfHumans.Text = "" + person.Count();



                    // Additional error checking is necessary here to prevent any null reference errors
                    switch (person.Count())
                    {
                        case 1:
                        {
                            me = person[0];
                            return;
                        }
                        case 2:
                        {
                            notMe = person[1];
                            return;
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.StackTrace);
            }
        }


        /**
         * This method initialises the Skeleton drawing for each person
         * */
        public void initialiseSkeletonDrawing(AllFramesReadyEventArgs e)
        {
            Skeleton me = null;
            Skeleton notMe = null;

            GetSkeleton(e, ref me, ref notMe);

            // If it hasn't detected the first person there is no point in continuing to detect
            // the second person
            if (me == null)
            {
                return;
            }

            GetXYD(me, person1);
           // person1.DrawPerson();


            // Make sure there's a second person of course
            if (notMe == null)
            {
                return; 
            }

            GetXYD(notMe, person2);
           // person2.DrawPerson();
        }

        public void GetXYD(Skeleton me, Person person)
        {
           // Additional null reference error checking although this
            // should not be necessary
            if (me == null || person == null)
                return;

            //head
            DepthImagePoint headDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.Head].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint headColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.Head].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jHead.X = headColorPoint.X;
            person.jHead.Y = headColorPoint.Y;
            person.jHead.D = headDepthPoint.Depth;


            // LEFT PALM
            DepthImagePoint LeftPalmDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.HandLeft].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint LeftPalmColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.HandLeft].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jHandLeft.X = LeftPalmColorPoint.X;
            person.jHandLeft.Y = LeftPalmColorPoint.Y;
            person.jHandLeft.D = LeftPalmDepthPoint.Depth;

            // RIGHT PALM
            DepthImagePoint RightPalmDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.HandRight].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint RightPalmColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.HandRight].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jHandRight.X = RightPalmColorPoint.X;
            person.jHandRight.Y = RightPalmColorPoint.Y;
            person.jHandRight.D = RightPalmDepthPoint.Depth;

            // LEFT ELBOW
            DepthImagePoint LeftElbowDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.ElbowLeft].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint LeftElbowColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.ElbowLeft].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jElbowLeft.X = LeftElbowColorPoint.X;
            person.jElbowLeft.Y = LeftElbowColorPoint.Y;
            person.jElbowLeft.D = LeftElbowDepthPoint.Depth;

            // RIGHT ELBOW
            DepthImagePoint RightElbowDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.ElbowRight].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint RightElbowColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.ElbowRight].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jElbowRight.X = RightElbowColorPoint.X;
            person.jElbowRight.Y = RightElbowColorPoint.Y;
            person.jElbowRight.D = RightElbowDepthPoint.Depth;

            // SPINE
            DepthImagePoint SpineDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.Spine].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint SpineColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.Spine].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jSpine.X = SpineColorPoint.X;
            person.jSpine.Y = SpineColorPoint.Y;
            person.jSpine.D = SpineDepthPoint.Depth;

            // RIGHT HIP
            DepthImagePoint jHipRightDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.HipRight].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jHipRightColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.HipRight].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jHipRight.X = jHipRightColorPoint.X;
            person.jHipRight.Y = jHipRightColorPoint.Y;
            person.jHipRight.D = jHipRightDepthPoint.Depth;

            // LEFT HIP
            DepthImagePoint jLeftHipDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.HipLeft].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jLeftHipColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.HipLeft].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jHipLeft.X = jLeftHipColorPoint.X;
            person.jHipLeft.Y = jLeftHipColorPoint.Y;
            person.jHipLeft.D = jLeftHipDepthPoint.Depth;

            // CENTER HIP
            DepthImagePoint jCenterHipDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.HipCenter].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jCenterHipColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.HipCenter].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jHipCenter.X = jCenterHipColorPoint.X;
            person.jHipCenter.Y = jCenterHipColorPoint.Y;
            person.jHipCenter.D = jCenterHipDepthPoint.Depth;

            // RIGHT KNEE
            DepthImagePoint jKneeRightDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.KneeRight].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jKneeRightColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.KneeRight].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jKneeRight.X = jKneeRightColorPoint.X;
            person.jKneeRight.Y = jKneeRightColorPoint.Y;
            person.jKneeRight.D = jKneeRightDepthPoint.Depth;

            // LEFT KNEE
            DepthImagePoint jKneeLeftDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.KneeLeft].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jKneeLeftColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.KneeLeft].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jKneeLeft.X = jKneeLeftColorPoint.X;
            person.jKneeLeft.Y = jKneeLeftColorPoint.Y;
            person.jKneeLeft.D = jKneeLeftDepthPoint.Depth;

            //  SHOULDER CENTER
            DepthImagePoint jShoulderCenterDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.ShoulderCenter].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jShoulderCenterColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.ShoulderCenter].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jShoulderCenter.X = jShoulderCenterColorPoint.X;
            person.jShoulderCenter.Y = jShoulderCenterColorPoint.Y;
            person.jShoulderCenter.D = jShoulderCenterDepthPoint.Depth;

            // SHOULDER LEFT
            DepthImagePoint jShoulderLeftDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.ShoulderLeft].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jShoulderLeftColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.ShoulderLeft].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jShoulderLeft.X = jShoulderLeftColorPoint.X;
            person.jShoulderLeft.Y = jShoulderLeftColorPoint.Y;
            person.jShoulderLeft.D = jShoulderLeftDepthPoint.Depth;

            // SHOULDER RIGHT
            DepthImagePoint jShoulderRightDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.ShoulderRight].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jShoulderRightColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.ShoulderRight].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jShoulderRight.X = jShoulderRightColorPoint.X;
            person.jShoulderRight.Y = jShoulderRightColorPoint.Y;
            person.jShoulderRight.D = jShoulderRightDepthPoint.Depth;

            // LEFT WRIST
            DepthImagePoint jWristLeftDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.WristLeft].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jWristLeftColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.WristLeft].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jWristLeft.X = jWristLeftColorPoint.X;
            person.jWristLeft.Y = jWristLeftColorPoint.Y;
            person.jWristLeft.D = jWristLeftDepthPoint.Depth;

            // RIGHT WRIST
            DepthImagePoint jWristRightDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.WristRight].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jWristRightColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.WristRight].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jWristRight.X = jWristRightColorPoint.X;
            person.jWristRight.Y = jWristRightColorPoint.Y;
            person.jWristRight.D = jWristRightDepthPoint.Depth;

            //  LEFT ANKLE
            DepthImagePoint jAnkleLeftDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.AnkleLeft].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jAnkleLeftColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.AnkleLeft].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jAnkleLeft.X = jAnkleLeftColorPoint.X;
            person.jAnkleLeft.Y = jAnkleLeftColorPoint.Y;
            person.jAnkleLeft.D = jAnkleLeftDepthPoint.Depth;

            //  RIGHT ANKLE
            DepthImagePoint jAnkleRightDepthPoint = sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
            (me.Joints[JointType.AnkleRight].Position, DepthImageFormat.Resolution640x480Fps30);

            ColorImagePoint jAnkleRightColorPoint = sensor.CoordinateMapper.MapSkeletonPointToColorPoint
            (me.Joints[JointType.AnkleRight].Position, ColorImageFormat.RgbResolution640x480Fps30);

            person.jAnkleRight.X = jAnkleRightColorPoint.X;
            person.jAnkleRight.Y = jAnkleRightColorPoint.Y;
            person.jAnkleRight.D = jAnkleRightDepthPoint.Depth;
        }

 

    }
}
